#!/usr/bin/python
#import cv2
import os

import numpy
import rospy

from std_msgs.msg import Header
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pcl2


def main():
    frame = 0
    rospy.init_node('kitti_node',anonymous=True)
    pcl_pub = rospy.Publisher('kitti_point_cloud', PointCloud2, queue_size=10)  #PointCloud2

    rate = rospy.Rate(5)
    while not rospy.is_shutdown():
        point_cloud = numpy.fromfile("bins/%06d.bin" % frame, dtype=numpy.float32).reshape(-1, 4)

        header = Header()
        header.stamp = rospy.Time.now()
        header.frame_id = 'map'

        pcl_pub.publish(pcl2.create_cloud_xyz32(header, point_cloud[:, :3]))
        rospy.loginfo("published")
        rate.sleep()
        frame += 1
        if frame >= 10:
            frame = 0

if __name__ == '__main__':
    main()
    
